#include "arttracking.h"
#include "body.h"


namespace Tracking
{
	ARTTracking::ARTTracking(int port)
		: TrackingDevice(DeviceType::ART),
		  _port(port)
	{
	
	
	
	}

	Body* ARTTracking::getBody(int id)
	{
		for(int i=0;i<_bodys.size();i++)
			if(_bodys[i]->getId()==id && _bodys[i]->getBodyType()==BodyType::BODY)
				return dynamic_cast<Body*>(_bodys[i]);
	}

	FlyStick* ARTTracking::getFlyStick(int id)
	{
		for(int i=0;i<_bodys.size();i++)
			if(_bodys[i]->getId()==id && _bodys[i]->getBodyType()==BodyType::FLYSTICK)
				return dynamic_cast<FlyStick*>(_bodys[i]);
		return NULL;
	}

	bool ARTTracking::init()
	{
		std::cout << "Initializing Tracker (Port: "<< _port << ")" << std::endl;

		//init DTrackSDK
		_dtrack = new DTrackSDK(_port);

		//check if running
		if( !_dtrack->isLocalDataPortValid() || !_dtrack->receive() ){
			std::cout << "DTrack init error" << std::endl;
			return false;
		}
		_running = true;


		//init bodys
		int bodyCount = _dtrack->getNumBody();
		for(int i = 0;i<bodyCount;i++){
			//get body from tracker
			DTrack_Body_Type_d body;
			body= *_dtrack->getBody(i);
			//store body in array
			Body *b = new Body(body.id);

			_bodys.push_back(b);
			b->_update(body);
		}

		int flyStickCount = _dtrack->getNumFlyStick();
		for(int i = 0;i<flyStickCount;i++){
			//get body from tracker
			DTrack_FlyStick_Type_d flystick;
			flystick = *_dtrack->getFlyStick(i);
			//store body in array
			FlyStick *f = new FlyStick(flystick.id);
			_bodys.push_back(f);
			f->_update(flystick);
		}
		
		_running = true;
		std::cout<<"ARTTracking initialized."<<std::endl;
		return true;
	}

	bool ARTTracking::update()
	{
		if(!_dtrack->receive())
			return false;


		// bodies:
		DTrack_Body_Type_d body;
		for(int i=0; i<_dtrack->getNumBody(); i++){
			body = *_dtrack->getBody(i);
			getBody(body.id)->_update(body);
		}

		//flysticks:
		DTrack_FlyStick_Type_d flystick;
		for(int i=0; i<_dtrack->getNumFlyStick(); i++){
			flystick = *_dtrack->getFlyStick(i);
			getFlyStick(flystick.id)->_update(flystick);
		}
		return true;

	}
	void ARTTracking::uninit()
	{
		delete _dtrack;
		_dtrack = 0;	
	}
}